#include <atomic>
#include <future>
#include <iostream>
#include <thread>
#include <cassert>

#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/param_server/param_server.h>
#include <mavsdk/plugins/param/param.h>
#include <mavsdk/plugins/telemetry_server/telemetry_server.h>
#include <mavsdk/plugins/action_server/action_server.h>
#include <mavsdk/plugins/mission_raw_server/mission_raw_server.h>
#include <mavsdk/plugins/mission/mission.h>

using namespace mavsdk;

using std::chrono::seconds;
using std::chrono::milliseconds;
using std::this_thread::sleep_for;

int main(int argc, char** argv)
{
    mavsdk::Mavsdk ground{mavsdk::Mavsdk::Configuration{mavsdk::ComponentType::GroundStation}};
    //mavsdk::Mavsdk ground{mavsdk::Mavsdk::Configuration{mavsdk::ComponentType::Autopilot}};

    auto result = ground.add_any_connection("udpin://0.0.0.0:14551");

    if (result == mavsdk::ConnectionResult::Success) {
        std::cout << "Connected autopilot server side!" << std::endl;
    }
    std::cout << "Conn Succ\n";

    auto prom = std::promise<std::shared_ptr<mavsdk::System>>{};
    auto fut = prom.get_future();
    
    Mavsdk::NewSystemHandle handle = ground.subscribe_on_new_system([&ground, &prom, &handle]() {
        auto system = ground.systems().back();

        if (system->has_autopilot()) {
            std::cout << "Discovered Autopilot from Client" << std::endl;

            // Unsubscribe again as we only want to find one system.
            ground.unsubscribe_on_new_system(handle);
            prom.set_value(system);
        } else {
            std::cout << "No MAVSDK found" << std::endl;
        }
    });

    if (fut.wait_for(std::chrono::seconds(10)) == std::future_status::timeout) {
        std::cout << "No autopilot found, exiting" << std::endl;
        return 1;
    }

    std::this_thread::sleep_for(std::chrono::seconds(1));

    auto system = fut.get();
/*
    auto system = ground.first_autopilot(10.0);
    assert(ground.systems().size() != 0);
    //assert(system);
    
*/
    auto telemetry = mavsdk::Telemetry{system};
    const Telemetry::Result set_rate_result = telemetry.set_rate_battery(1.0);
    assert(set_rate_result);  

    telemetry.subscribe_battery([](Telemetry::Battery battery) {
        //std::cout << "FF\n";
        std::cout << battery << std::endl;
    });

/*
    auto server_component = mavsdkTester.server_component();

    auto paramServer = mavsdk::ParamServer{server_component};
    auto telemServer = mavsdk::TelemetryServer{server_component};
    auto actionServer = mavsdk::ActionServer{server_component};

    paramServer.provide_param_int("MIS_TAKEOFF_ALT", 0);
    paramServer.provide_param_int("MY_PARAM", 1);

    actionServer.set_allowable_flight_modes({true, true, true});
    actionServer.set_allow_takeoff(true);
    actionServer.set_armable(true, true);

    TelemetryServer::Position position{55.953251, -3.188267, 0, 0};
    TelemetryServer::PositionVelocityNed positionVelocityNed{{0, 0, 0}, {0, 0, 0}};
    TelemetryServer::VelocityNed velocity{};
    TelemetryServer::Heading heading{60};
    TelemetryServer::RawGps rawGps{
        0, 55.953251, -3.188267, 0, NAN, NAN, 0, NAN, 0, 0, 0, 0, 0, 0};
    TelemetryServer::GpsInfo gpsInfo{11, TelemetryServer::FixType::Fix3D};
    TelemetryServer::Battery battery;

    telemServer.publish_home(position);

    actionServer.subscribe_takeoff([&position](ActionServer::Result result, bool takeoff) {
        if (result == ActionServer::Result::Success) {
            position.relative_altitude_m = 10;
        }
    });

    actionServer.subscribe_land([&position](ActionServer::Result result, bool land) {
        if (result == ActionServer::Result::Success) {
            position.relative_altitude_m = 0;
        }
    });

    while (true) {
        std::this_thread::sleep_for(std::chrono::seconds(1));

        telemServer.publish_home(position);
        telemServer.publish_sys_status(battery, true, true, true, true, true);
        telemServer.publish_position(position, velocity, heading);
        telemServer.publish_position_velocity_ned(positionVelocityNed);
        telemServer.publish_raw_gps(rawGps, gpsInfo);

        telemServer.publish_unix_epoch_time(42);
    }
*/
    while(true)
      ;//std::cout << "FF\n";

    return 0;
}
